Program Listing for File tf2eigen.hpp

↰ Return to documentation for file (codes/cubicle_merge/tf2eigen.hpp)

#pragma once

#include "../../common/utils/macros.hpp"
#include <eigen3/Eigen/Geometry>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Matrix3x3.h>

inline Eigen::Vector3d tf2eigen(const tf2::Vector3& v)
{
  return Eigen::Vector3d(v.x(), v.y(), v.z());
}

inline Eigen::Matrix3d tf2eigen(const tf2::Matrix3x3& m)
{
  Eigen::Matrix3d ret;

  forn(i, 3) forn(j, 3) ret(i, j) = m[i][j];

  return ret;
}